import rclpy
from rclpy.node import Node
import cv2
import numpy as np
from sensor_msgs.msg import Image,PointCloud2
from cv_bridge import CvBridge
from std_msgs.msg import Float32MultiArray
from nav_msgs.msg import Path
from geometry_msgs.msg import Point,Quaternion,PoseStamped
from .odometry import Binocular_SLAM
from .utils import draw_bino,create_pointcloud
class Binocular(Node):
    def __init__(self,node_name):
        super().__init__(node_name=node_name)
        self.get_logger().info(f"start bino_sub:{node_name}")
        
        self.bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        self.ORB = cv2.ORB_create()
        self.MATCHER = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
        self.bridge = CvBridge()
        
        self.lk = None
        self.rk = None
        self.limg = None
        self.rimg = None

        self.lk_sub = self.create_subscription(Float32MultiArray,'lK',self.callback_lK,1)
        self.rk_sub = self.create_subscription(Float32MultiArray,'rK',self.callback_rK,1)
        self.left_sub = self.create_subscription(Image,'left_img',self.callback_limg,10)
        self.right_sub = self.create_subscription(Image,'right_img',self.callback_rimg,10)
        
        self.bino_slam = Binocular_SLAM()
        self.path = Path()
        self.path.header.frame_id = 'map'

        self.frame_match_pub = self.create_publisher(Image,'result',10)
        self.path_pub = self.create_publisher(Path,'lpath',10)
        self.map_pub = self.create_publisher(PointCloud2,'map',10)
        
    def callback_lK(self,k):
        np_array = np.array(k.data, dtype=np.float32)
        self.lk = np.reshape(np_array,(3,3))
        # print(self.lk)
    def callback_rK(self,k):
        np_array = np.array(k.data, dtype=np.float32)
        self.rk = np.reshape(np_array,(3,3))
        # print(self.rk)

    def callback_limg(self,img):
        cv_image = self.bridge.imgmsg_to_cv2(img, desired_encoding="bgr8")
        self.limg = cv_image
        self.process_frame()

    def callback_rimg(self,img):
        cv_image = self.bridge.imgmsg_to_cv2(img, desired_encoding="bgr8")
        self.rimg = cv_image
        self.process_frame()

    def process_frame(self):
        # print(self.lk)
        # print(self.rk)
        if self.limg is not None and self.rimg is not None and self.lk is not None and self.rk is not None:            
            self.bino_slam.update((self.limg,self.rimg))
            self.bino_slam.compute_xyz_camera()
            path1 = self.bino_slam.lpath
            path2 = self.bino_slam.rpath
            path1 = np.array(path1)
            path2 = np.array(path2)
            l_img,r_img = draw_bino(self.limg,self.bino_slam.prev_lkps,self.rimg,self.bino_slam.prev_rkps)
            img = np.hstack((l_img,r_img))
            pose = PoseStamped()

            point = Point()
            point.x, point.y, point.z = float(self.bino_slam.lpose[0,3]),float(self.bino_slam.lpose[1,3]),float(self.bino_slam.lpose[2,3])
            pose.pose.position = point
            
            quat = Quaternion()
            quat.x,quat.y,quat.z,quat.w = 0.0,0.0,0.0,1.0
            pose.pose.orientation = quat
            self.path.header.stamp = self.get_clock().now().to_msg()
            
            self.path.poses.append(pose)
            self.path_pub.publish(self.path)
            
            pointscloud = create_pointcloud(self.bino_slam.map_pts)
            self.map_pub.publish(pointscloud)
            self.frame_match_pub.publish(self.bridge.cv2_to_imgmsg(img,'bgr8'))
            self.limg = None
            self.rimg = None
def main(args=None):
    rclpy.init(args=args)
    node = Binocular(node_name='Bino_slam_node')
    rclpy.spin(node) 
    rclpy.shutdown()
if __name__ == '__main__':
    main()